Loading from the ekf input file

In [60]:
import plotly.offline as py
from plotly.graph_objs import *
import pandas as pd
import math
py.init_notebook_mode()

my_cols=['c1','c2','c3','c4','c5']
with open('obj_pose-laser-radar-ekf-input.txt') as f:
    table = pd.read_table(f, sep='\t', header=None, names=my_cols, lineterminator='\n')
    
table_vis_in = pd.DataFrame();

def f_x(row):
    if(row[0]=='L'):
        return row[1];
    else:
        ro = row[1]
        theta = row[2]
        return ro*math.cos(theta)

def f_y(row):
    if(row[0]=='L'):
        return row[2];
    else:
        ro = row[1]
        theta = row[2]
        return ro*math.sin(theta)

table_vis_in['c1'] = table['c1']
table_vis_in['c2'] = table.apply(f_x, axis=1)
table_vis_in['c3'] = table.apply(f_y, axis=1)
In [61]:
table_vis_in
Out[61]:
c1 c2 c3
0 1.00000 0.540302 0.841471
1 8.43507 0.203618 0.147440
2 8.43579 0.383520 -0.014152
3 8.44459 0.263796 0.001482
4 8.28180 0.333551 -0.486705
5 8.32155 0.189047 -0.246337
6 7.96636 -0.051043 -0.146397
7 8.02533 -0.055651 -0.154761
8 7.66441 -0.153152 -0.209927
9 7.74437 -0.119129 -0.160887
10 7.42867 -0.082057 -0.131512
11 7.52352 -0.059307 -0.093461
12 7.13994 -0.048162 -0.061602
13 7.24079 -0.019023 -0.023809
14 6.90236 0.042083 0.061222
15 7.00533 0.074658 0.106419
16 6.67715 0.141404 0.219091
17 6.77134 0.170403 0.258512
18 6.51966 0.229369 0.494655
19 6.61283 0.233087 0.494129
20 6.37746 0.245672 0.823973
21 6.47435 0.254007 0.828337
22 6.18250 0.308252 1.077640
23 6.27780 0.311234 1.053904
24 6.09792 0.056763 1.692068
25 6.19847 0.040666 1.597302
26 5.97969 -0.069848 2.012328
27 6.07190 -0.087555 1.910915
28 5.91544 -0.495656 2.457687
29 6.01922 -0.534285 2.285408
... ... ... ...
1194 8.82561 2.251870 -4.913542
1195 8.77293 2.468239 -4.877004
1196 9.13564 2.469971 -4.548525
1197 9.06329 2.637429 -4.547817
1198 9.38491 2.291551 -4.406135
1199 9.31071 2.440105 -4.409256
1200 9.64870 2.241010 -4.178253
1201 9.56624 2.366731 -4.199810
1202 9.90296 2.155826 -3.971155
1203 9.81410 2.263861 -4.008545
1204 10.19080 2.289557 -3.633839
1205 10.09840 2.399011 -3.668440
1206 10.38240 1.825983 -3.652914
1207 10.28920 1.907355 -3.701618
1208 10.61500 1.703202 -3.454407
1209 10.51810 1.775420 -3.514948
1210 10.82750 1.500249 -3.299267
1211 10.73260 1.564164 -3.355224
1212 10.98720 1.108582 -3.213592
1213 10.90210 1.147497 -3.265375
1214 11.18800 1.016903 -2.984767
1215 11.09300 1.058783 -3.052786
1216 11.34470 0.686857 -2.827263
1217 11.25070 0.710752 -2.895688
1218 11.39140 0.010016 -2.682551
1219 11.30600 -0.007278 -2.739730
1220 11.49600 -0.180427 -2.416504
1221 11.40760 -0.200779 -2.476825
1222 11.60430 -0.310763 -2.118872
1223 11.51870 -0.335494 -2.176120

1224 rows × 3 columns

Loading from the GT input file

In [52]:
import plotly.offline as py
from plotly.graph_objs import *
import pandas as pd
import math
py.init_notebook_mode()

my_cols=['c1','c2','c3','c4','c5','c5','c5','c5']
with open('obj_pose-laser-radar-.txt') as f:
    table_gt = pd.read_table(f, sep='\t', header=None, names=my_cols, lineterminator='\n')
    
table_gt
Out[52]:
c1 c2 c3 c4 c5
R 8.60 2.500000e-01 -3.000290 0.000000 1477010443399637
L 8.45 2.500000e-01 -3.000270 0.000000 1477010443449633
R 8.45 2.500000e-01 0.000000 0.000000 1477010443499690
L 8.45 2.500000e-01 0.000000 0.000000 1477010443549747
R 8.35 2.500000e-01 -1.819790 0.000000 1477010443604698
L 8.25 2.500000e-01 -1.819780 0.000000 1477010443659650
R 8.05 2.000000e-01 -3.999760 -0.999940 1477010443709653
L 7.85 1.500000e-01 -3.999720 -0.999930 1477010443759657
R 7.70 1.500000e-01 -2.999820 0.000000 1477010443809660
L 7.55 1.500000e-01 -2.999820 0.000000 1477010443859663
R 7.45 1.000010e-01 -1.816500 -0.908239 1477010443914714
L 7.35 5.000110e-02 -1.816480 -0.908230 1477010443969766
R 7.20 9.499490e-07 -2.728510 -0.909507 1477010444024741
L 7.05 -4.999920e-02 -2.728490 -0.909499 1477010444079717
R 6.95 -1.500000e-01 -1.818980 -1.818980 1477010444134693
L 6.85 -2.500000e-01 -1.818960 -1.818970 1477010444189670
R 6.75 -3.499990e-01 -2.000760 -2.000750 1477010444239651
L 6.65 -4.499990e-01 -2.000760 -2.000750 1477010444289632
R 6.60 -5.499990e-01 -0.998836 -1.997690 1477010444339690
L 6.55 -6.500000e-01 -0.998840 -1.997690 1477010444389748
R 6.50 -8.500000e-01 -0.908747 -3.634980 1477010444444769
L 6.45 -1.050000e+00 -0.908739 -3.634940 1477010444499791
R 6.35 -1.150000e+00 -1.818970 -1.818980 1477010444554767
L 6.25 -1.250000e+00 -1.818960 -1.818960 1477010444609744
R 6.30 -1.450000e+00 0.909739 -3.638940 1477010444664705
L 6.35 -1.650000e+00 0.909727 -3.638910 1477010444719667
R 6.30 -1.800000e+00 -0.999494 -2.998500 1477010444769692
L 6.25 -1.950000e+00 -0.999489 -2.998470 1477010444819718
R 6.30 -2.100000e+00 0.999984 -2.999940 1477010444869719
L 6.35 -2.250000e+00 0.999979 -2.999940 1477010444919720
... ... ... ... ... ...
R 9.15 -5.450000e+00 1.819160 1.819180 1477010507134822
L 9.25 -5.350000e+00 1.819160 1.819160 1477010507189793
R 9.35 -5.300000e+00 1.818950 0.909466 1477010507244770
L 9.45 -5.250000e+00 1.818940 0.909470 1477010507299747
R 9.55 -5.100000e+00 1.819860 2.729810 1477010507354696
L 9.65 -4.950000e+00 1.819870 2.729810 1477010507409645
R 9.75 -4.850000e+00 1.817360 1.817350 1477010507464670
L 9.85 -4.750000e+00 1.817360 1.817350 1477010507519695
R 9.95 -4.650000e+00 1.818060 1.818050 1477010507574699
L 10.05 -4.550000e+00 1.818050 1.818050 1477010507629703
R 10.20 -4.450000e+00 2.726090 1.817400 1477010507684727
L 10.35 -4.350000e+00 2.726060 1.817370 1477010507739752
R 10.40 -4.200000e+00 0.908945 2.726880 1477010507794760
L 10.45 -4.050000e+00 0.908953 2.726870 1477010507849768
R 10.55 -3.950000e+00 1.819030 1.819040 1477010507904742
L 10.65 -3.850000e+00 1.819020 1.819030 1477010507959717
R 10.75 -3.700000e+00 1.819310 2.728960 1477010508014683
L 10.85 -3.550000e+00 1.819310 2.728960 1477010508069649
R 10.90 -3.400000e+00 0.999745 2.999280 1477010508119661
L 10.95 -3.250000e+00 0.999754 2.999280 1477010508169673
R 11.05 -3.150000e+00 1.817640 1.817650 1477010508224689
L 11.15 -3.050000e+00 1.817630 1.817640 1477010508279706
R 11.20 -2.900000e+00 0.909657 2.728960 1477010508334672
L 11.25 -2.750000e+00 0.909648 2.728930 1477010508389639
R 11.20 -2.600000e+00 -0.998725 2.996170 1477010508439703
L 11.15 -2.450000e+00 -0.998725 2.996160 1477010508489767
R 11.20 -2.300000e+00 0.908813 2.726430 1477010508544784
L 11.25 -2.150000e+00 0.908805 2.726400 1477010508599802
R 11.30 -2.000000e+00 0.909855 2.729560 1477010508654756
L 11.35 -1.850000e+00 0.909847 2.729530 1477010508709711

1224 rows × 5 columns

Loading from the ekf output file

In [53]:
import plotly.offline as py
from plotly.graph_objs import *
import pandas as pd
import math
py.init_notebook_mode()

my_cols=['c1','c2','c3','c4']
with open('obj_pose-laser-radar-ekf-output.txt') as f:
    table_ekf_output = pd.read_table(f, sep='\t', header=None, names=my_cols, lineterminator='\n')
    
table_ekf_output
Out[53]:
c1 c2 c3 c4
0 1.83576 -0.032431 1.00000 0.999998
1 2.92748 0.394506 1.00055 1.000210
2 4.12700 0.420077 1.00235 1.000250
3 5.35930 0.479464 1.00606 1.000430
4 6.59769 0.697960 1.01226 1.001520
5 7.83177 0.889842 1.02152 1.002960
6 9.09016 1.179420 1.03474 1.006010
7 10.31640 1.493580 1.05192 1.010410
8 11.56050 1.850390 1.07433 1.016830
9 12.80130 2.250110 1.10226 1.025830
10 14.10000 2.668990 1.13799 1.037360
11 15.35960 3.135620 1.17957 1.052760
12 16.64810 3.607480 1.22984 1.071170
13 17.93290 4.139980 1.28832 1.095410
14 19.21370 4.663630 1.35559 1.122910
15 20.53650 5.228890 1.43498 1.156830
16 21.85200 5.813800 1.52445 1.196620
17 23.14880 6.399170 1.62368 1.241410
18 24.46300 6.988100 1.73607 1.291770
19 25.78450 7.570610 1.86164 1.347120
20 27.09160 8.164650 1.99892 1.409510
21 28.38460 8.739820 2.14829 1.475960
22 29.69700 9.325430 2.31434 1.550050
23 31.02940 9.891400 2.49824 1.628170
24 32.32970 10.442400 2.69332 1.710830
25 33.62360 10.958500 2.90361 1.794720
26 34.91280 11.449500 3.12989 1.880900
27 36.19230 11.924900 3.37177 1.970760
28 37.45360 12.368600 3.62784 2.060850
29 38.71210 12.793600 3.90160 2.153290
... ... ... ... ...
69 75.36310 15.547800 26.63900 2.634440
70 76.23860 15.556700 27.72670 2.645510
71 77.12630 15.578800 28.86110 2.673760
72 78.02770 15.615200 30.04560 2.721590
73 78.93570 15.667900 31.27180 2.792740
74 79.85640 15.734700 32.54920 2.885440
75 80.79670 15.820400 33.88900 3.007450
76 81.74570 15.921300 35.27730 3.155040
77 82.69830 16.033900 36.70760 3.324150
78 83.66150 16.157500 38.19130 3.514540
79 84.63520 16.300400 39.72960 3.740290
80 85.61720 16.457600 41.32040 3.994930
81 86.61030 16.623900 42.96930 4.271140
82 87.61050 16.799700 44.67090 4.570200
83 88.61500 16.984800 46.42160 4.892890
84 89.62790 17.182400 48.22950 5.245420
85 90.64820 17.381100 50.09390 5.608620
86 91.67000 17.581800 52.00500 5.984000
87 92.70180 17.790200 53.97950 6.382870
88 93.73930 17.998900 56.01080 6.791480
89 94.77670 18.203400 58.08790 7.200890
90 95.81420 18.409900 60.21190 7.623570
91 96.85430 18.619200 62.38840 8.061690
92 97.89460 18.819800 64.61340 8.490580
93 98.93510 19.018700 66.88700 8.925420
94 99.97560 19.208500 69.20950 9.348960
95 101.01300 19.393600 71.57390 9.770880
96 102.04500 19.573300 73.97750 10.189200
97 103.07200 19.745800 76.41780 10.599100
98 104.09400 19.908900 78.89470 10.994500

99 rows × 4 columns

In [54]:
table_ekf_output[0:10]
Out[54]:
c1 c2 c3 c4
0 1.83576 -0.032431 1.00000 0.999998
1 2.92748 0.394506 1.00055 1.000210
2 4.12700 0.420077 1.00235 1.000250
3 5.35930 0.479464 1.00606 1.000430
4 6.59769 0.697960 1.01226 1.001520
5 7.83177 0.889842 1.02152 1.002960
6 9.09016 1.179420 1.03474 1.006010
7 10.31640 1.493580 1.05192 1.010410
8 11.56050 1.850390 1.07433 1.016830
9 12.80130 2.250110 1.10226 1.025830
In [55]:
table_gt[0:10]
Out[55]:
c1 c2 c3 c4 c5
R 8.60 0.25 -3.00029 0.00000 1477010443399637
L 8.45 0.25 -3.00027 0.00000 1477010443449633
R 8.45 0.25 0.00000 0.00000 1477010443499690
L 8.45 0.25 0.00000 0.00000 1477010443549747
R 8.35 0.25 -1.81979 0.00000 1477010443604698
L 8.25 0.25 -1.81978 0.00000 1477010443659650
R 8.05 0.20 -3.99976 -0.99994 1477010443709653
L 7.85 0.15 -3.99972 -0.99993 1477010443759657
R 7.70 0.15 -2.99982 0.00000 1477010443809660
L 7.55 0.15 -2.99982 0.00000 1477010443859663
In [57]:
import plotly.offline as py
from plotly.graph_objs import *


#estimations
trace1 = Scatter(
    x=table_ekf_output['c1'],
    y=table_ekf_output['c2'],
    xaxis='x2',
    yaxis='y2'
)

#ground truth
trace2 = Scatter(
    x=table_gt['c1'],
    y=table_gt['c2'],
    xaxis='x2',
    yaxis='y2'
)

data = [trace1,trace2]

layout = Layout(
    xaxis2=dict(
   
        anchor='y2'
    ),
    yaxis2=dict(
    
        anchor='x2'
    )
)

layout = Layout(
    paper_bgcolor='rgb(255,255,255)',
    plot_bgcolor='rgb(229,229,229)',
    xaxis=XAxis(
        gridcolor='rgb(255,255,255)',
        range=[1,10],
        showgrid=True,
        showline=False,
        showticklabels=True,
        tickcolor='rgb(127,127,127)',
        ticks='outside',
        zeroline=False
    ),
    yaxis=YAxis(
        gridcolor='rgb(255,255,255)',
        showgrid=True,
        showline=False,
        showticklabels=True,
        tickcolor='rgb(127,127,127)',
        ticks='outside',
        zeroline=False
    ),
)
fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')
In [48]:
x = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
x_rev = x[::-1]

# Line 1
y1 = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
y1_upper = [2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
y1_lower = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
y1_lower = y1_lower[::-1]

# Line 2
y2 = [5, 2.5, 5, 7.5, 5, 2.5, 7.5, 4.5, 5.5, 5]
y2_upper = [5.5, 3, 5.5, 8, 6, 3, 8, 5, 6, 5.5]
y2_lower = [4.5, 2, 4.4, 7, 4, 2, 7, 4, 5, 4.75]
y2_lower = y2_lower[::-1]

# Line 3
y3 = [10, 8, 6, 4, 2, 0, 2, 4, 2, 0]
y3_upper = [11, 9, 7, 5, 3, 1, 3, 5, 3, 1]
y3_lower = [9, 7, 5, 3, 1, -.5, 1, 3, 1, -1]
y3_lower = y3_lower[::-1]

trace1 = Scatter(
    x=x+x_rev,
    y=y1_upper+y1_lower,
    fill='tozerox',
    fillcolor='rgba(0,100,80,0.2)',
    line=Line(color='transparent'),
    showlegend=False,
    name='Fair',
)
trace2 = Scatter(
    x=x+x_rev,
    y=y2_upper+y2_lower,
    fill='tozerox',
    fillcolor='rgba(0,176,246,0.2)',
    line=Line(color='transparent'),
    name='Premium',
    showlegend=False,
)
trace3 = Scatter(
    x=x+x_rev,
    y=y3_upper+y3_lower,
    fill='tozerox',
    fillcolor='rgba(231,107,243,0.2)',
    line=Line(color='transparent'),
    showlegend=False,
    name='Fair',
)
trace4 = Scatter(
    x=x,
    y=y1,
    line=Line(color='rgb(0,100,80)'),
    mode='lines',
    name='Fair',
)
trace5 = Scatter(
    x=x,
    y=y2,
    line=Line(color='rgb(0,176,246)'),
    mode='lines',
    name='Premium',
)
trace6 = Scatter(
    x=x,
    y=y3,
    line=Line(color='rgb(231,107,243)'),
    mode='lines',
    name='Ideal',
)

data = Data([trace1, trace2, trace3, trace4, trace5, trace6])

layout = Layout(
    paper_bgcolor='rgb(255,255,255)',
    plot_bgcolor='rgb(229,229,229)',
    xaxis=XAxis(
        gridcolor='rgb(255,255,255)',
        range=[1,10],
        showgrid=True,
        showline=False,
        showticklabels=True,
        tickcolor='rgb(127,127,127)',
        ticks='outside',
        zeroline=False
    ),
    yaxis=YAxis(
        gridcolor='rgb(255,255,255)',
        showgrid=True,
        showline=False,
        showticklabels=True,
        tickcolor='rgb(127,127,127)',
        ticks='outside',
        zeroline=False
    ),
)
fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'shaded_lines')
In [ ]:
 
In [ ]:
 
In [ ]:
 
In [ ]: